#include <ace/config.h>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <sstream>
#include <fstream>
#include <sys/stat.h>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>

#include <cv.h>
#include <cvaux.h>
#include <highgui.h>

#define MAX_COUNT 100
#define WIN_SIZE 10
#define FLOW_RATIO 80
#define FEAT_QLTY 0.01
#define FEAT_MIN_DIST 5.0
#define TRESHOLD 40
#define NUM_X 9
#define NUM_Y 6
#define WAIT_TIME 100
#define DEF 0
#define PI 3.14159265

using namespace std;

using namespace yarp::dev;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;

bool fileExists(string strFilename);

int main(int argc, char *argv[])
{  
  ImageOf<PixelRgb> tempYImage;
  IplImage *tempCImage;    
  IplImage *leftImg, *rightImg, *leftGray, *rightGray;
  IplImage *gray;
  IplImage *diff;
  IplImage *result3DImage, *imageX, *imageY, *imageZ;
  IplImage *result2DPoints;
  int displayCorners = 0;
  int showUndistorted = 1;
  const int maxScale = 1;
  const float squareSize = 24.0;
  int n, N;
  int count;
  int result;
  int i, j, k;
  int nframes;
  CvSize imageSize = {0, 0};
  
  vector<CvPoint3D32f> objectPoints;
  vector<CvPoint2D32f> points[2];
  vector<int> npoints;
  vector<uchar> active[2];
  vector<CvPoint2D32f> temp;
  
  bool flag;
  long begin;
  istringstream dataElement;
  
  CvStereoBMState *bmState = cvCreateStereoBMState();
  
  double m1[3][3], m2[3][3], d1[5], d2[5], rot[3][3], trans[3], ess[3][3], fund[3][3];
  double rotX[3][3], rotY[3][3], rotZ[3][3];
  double r1[3][3], r2[3][3], p1[3][4], p2[3][4], q[4][4];
  double d1Zeros[5], d2Zeros[5];
  CvMat _m1 = cvMat(3, 3, CV_64F, m1);
  CvMat _m2 = cvMat(3, 3, CV_64F, m2);
  CvMat _d1 = cvMat(1, 5, CV_64F, d1);
  CvMat _d2 = cvMat(1, 5, CV_64F, d2);
  CvMat _rot = cvMat(3, 3, CV_64F, rot);
  CvMat _rotX = cvMat(3, 3, CV_64F, rotX);
  CvMat _rotY = cvMat(3, 3, CV_64F, rotY);
  CvMat _rotZ = cvMat(3, 3, CV_64F, rotZ);
  CvMat _trans = cvMat(3, 1, CV_64F, trans);
  CvMat _ess = cvMat(3, 3, CV_64F, ess);
  CvMat _fund = cvMat(3, 3, CV_64F, fund);
  CvMat _r1 = cvMat(3, 3, CV_64F, r1);
  CvMat _r2 = cvMat(3, 3, CV_64F, r2);
  CvMat _p1 = cvMat(3, 4, CV_64F, p1);
  CvMat _p2 = cvMat(3, 4, CV_64F, p2);
  CvMat _q = cvMat(4, 4, CV_64F, q);
  CvMat _d1Zeros = cvMat(1, 5, CV_64F, d1Zeros);
  CvMat _d2Zeros = cvMat(1, 5, CV_64F, d2Zeros);
  
  
  CvMat part;
  
  ofstream disparityFile;
  
  n = NUM_X * NUM_Y;
  N = 0;
  temp.resize(n, cvPoint2D32f(0, 0));
  
  cvNamedWindow("OriginalLeft", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("GrayLeft", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("OriginalRight", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("GrayRight", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Disparity", CV_WINDOW_AUTOSIZE);
  
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/Left0.ppm");
      
  // Convert the image in a openCv image
  tempCImage = cvCreateImage(cvSize(tempYImage.width(),  
			     tempYImage.height()), 
			     IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), tempCImage, CV_RGB2BGR);
  
  gray = cvCreateImage(cvGetSize(tempCImage), 8, 1);
  
  imageSize = cvGetSize(tempCImage);    int numberOfDisparities = imageSize.width / 8;
  numberOfDisparities -= numberOfDisparities % 16;

  //bmState->roi1 = roi1;
  //bmState->roi2 = roi2;
  bmState->preFilterCap = 31;
  bmState->SADWindowSize = 15;
  bmState->minDisparity = 0;
  bmState->numberOfDisparities = numberOfDisparities;
  bmState->textureThreshold = 10;
  bmState->uniquenessRatio = 15;
  bmState->speckleWindowSize = 100;
  bmState->speckleRange = 32;
  bmState->disp12MaxDiff = 1;
  
  cvCvtColor(tempCImage, gray, CV_BGR2GRAY);
  
  CvMat *mx1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *my1 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *mx2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *my2 = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  
  IplImage *mapXDis = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapYDis = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapXDis2 = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *mapYDis2 = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  IplImage *tempDistImage;
  
  cvSetIdentity(&_m1);
  cvSetIdentity(&_m2);
  cvZero(&_d1);
  cvZero(&_d2);
  cvZero(&_d1Zeros);
  cvZero(&_d2Zeros);
  
  m1[0][0] = 251.103;
  m1[1][1] = 248.425;
  m1[0][2] = 136.058;
  m1[1][2] = 115.946;
  m2[0][0] = 257.023;
  m2[1][1] = 254.647;
  m2[0][2] = 177.269;
  m2[1][2] = 125.096;

  d1[0] = -0.457156;
  d1[1] = 0.303359;
  d1[2] = -0.00273465;
  d1[3] = 0.00188351;
  d1[4] = -0.13247;
  d2[0] = -0.446377;
  d2[1] = 0.247249;
  d2[2] = 0.000347644;
  d2[3] = -0.000379765;
  d2[4] = -0.0884467;

  // old datas

//  m1[0][0] = 258.245;
//  m1[1][1] = 257.123;
//  m1[0][2] = 157.631;
//  m1[1][2] = 113.148;
//  m2[0][0] = 257.2;
//  m2[1][1] = 257.436;
//  m2[0][2] = 155.086;
//  m2[1][2] = 130.77;

//  d1[0] = -0.425346;
//  d1[1] = 0.146858;
//  d1[2] = -0.00271551;
//  d1[3] = 0.00490219;
//  d1[4] = 0;
//  d2[0] = -0.41324;
//  d2[1] = 0.158982;
//  d2[2] = -0.00148367;
//  d2[3] = 0.000609254;
//  d2[4] = 0;

  // end old datas
  
  cvInitUndistortMap(&_m1, &_d1, mapXDis, mapYDis);
  cvInitUndistortMap(&_m2, &_d2, mapXDis2, mapYDis2);
  
  // calculate retification parameter
  cout << "Calculating retification parameters..." << endl; 
  
//   float angle = 5.7;
  float angle = 0;
  
  rotX[0][0] = 1.0;
  rotX[0][1] = 0.0;
  rotX[0][2] = 0.0;
  rotX[1][0] = 0.0;
  rotX[1][1] = cos(angle*PI/180);
  rotX[1][2] = sin(angle*PI/180);
  rotX[2][0] = 0.0;
  rotX[2][1] = -sin(angle*PI/180);
  rotX[2][2] = cos(angle*PI/180);
  
//   angle = 20;
  angle = 0;
  
  trans[0] = -68 * cos(angle / 2.0 * PI / 180);
  trans[1] = 0.1;
  trans[2] = -68 * sin(angle / 2.0 * PI / 180);

  rotY[0][0] = cos(angle*PI/180);
  rotY[0][1] = 0.0;
  rotY[0][2] = -sin(angle*PI/180);
  rotY[1][0] = 0.0;
  rotY[1][1] = 1.0;
  rotY[1][2] = 0.0;
  rotY[2][0] = sin(angle*PI/180);
  rotY[2][1] = 0.0;
  rotY[2][2] = cos(angle*PI/180);
  
//   angle = 0;
  angle = 0;
  
  rotZ[0][0] = cos(angle*PI/180);
  rotZ[0][1] = sin(angle*PI/180);
  rotZ[0][2] = 0.0;
  rotZ[1][0] = -sin(angle*PI/180);
  rotZ[1][1] = cos(angle*PI/180);
  rotZ[1][2] = 0.0;
  rotZ[2][0] = 0.0;
  rotZ[2][1] = 0.0;
  rotZ[2][2] = 1.0;
  
  CvMat *rotzy = cvCreateMat( 3, 3, CV_64FC1);
  
  cvMatMul(&_rotZ, &_rotY, rotzy);
  cvMatMul(rotzy, &_rotX, &_rot);
  
  
  // end parameters "a manina"

  // parametri estrinsechi fissi
  trans[0] = -70.8398;
  trans[1] = 1.55403;
  trans[2] = 6.03106;

  rot[0][0] = 0.999645;
  rot[0][1] = -0.00800069;
  rot[0][2] = -0.0254203;
  rot[1][0] = 0.00792942;
  rot[1][1] = 0.999964;
  rot[1][2] = -0.00290345;
  rot[2][0] = 0.0254426;
  rot[2][1] = 0.00270085;
  rot[2][2] = 0.999673;
//   end parametri estrinsechi fissi
  
  cvStereoRectify(&_m1, &_m2, &_d1Zeros, &_d2Zeros,
		  imageSize, 
		  &_rot, &_trans,
		  &_r1, &_r2, &_p1, &_p2, &_q,
		  0);
  
  cvInitUndistortRectifyMap(&_m1, &_d1Zeros, &_r1, &_p1, mx1, my1);
  cvInitUndistortRectifyMap(&_m2, &_d2Zeros, &_r2, &_p2, mx2, my2);
  
  // load stereo images
  cout << "Loading stereo images" << endl;
  
  // left
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/Left0.ppm");
      
  // Convert the image in a openCv image
  leftImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
				 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), leftImg, CV_RGB2BGR);//   tempDistImage = cvCloneImage(leftGray);
//   cvRemap(tempDistImage, leftGray, mapXDis, mapYDis);
//   tempDistImage = cvCloneImage(rightGray);
//   cvRemap(tempDistImage, rightGray, mapXDis2, mapYDis2);
  
  imageSize = cvGetSize(leftImg);
  
  leftGray = cvCreateImage(cvGetSize(leftImg), 8, 1);
  cvCvtColor(leftImg, leftGray, CV_BGR2GRAY);
  
  // right
  yarp::sig::file::read(tempYImage, "/home/nigno/Immagini/dataset/Right0.ppm");
  
  // Convert the image in a openCv image
  rightImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
				 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), rightImg, CV_RGB2BGR);
  
  rightGray = cvCreateImage(cvGetSize(rightImg), 8, 1);
  
  cvCvtColor(rightImg, rightGray, CV_BGR2GRAY);
  
  // Undistortion
  
  // invert
//   for(i = 0; i < 5; i++)
//   {
//     d1[i] = -d1[i];
//     d2[i] = -d2[i];
//   }
  
  tempDistImage = cvCloneImage(leftGray);
  cvRemap(tempDistImage, leftGray, mapXDis, mapYDis);
  tempDistImage = cvCloneImage(rightGray);
  cvRemap(tempDistImage, rightGray, mapXDis2, mapYDis2);
  
  CvMat *img1r = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  CvMat *img2r = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  
  CvMat *disp = cvCreateMat(imageSize.height, imageSize.width, CV_32F);
  CvMat *vdisp = cvCreateMat(imageSize.height, imageSize.width, CV_8U);
  CvMat *pair = cvCreateMat(imageSize.height, imageSize.width * 2, CV_8UC3);
  
  result3DImage = cvCreateImage(cvGetSize(leftImg), IPL_DEPTH_32F, 3);
  result2DPoints = cvCreateImage(cvGetSize(leftImg), IPL_DEPTH_32F, 3);

  // set correspondence parameters
  assert(bmState != 0);
  if (DEF)
  {
    bmState->preFilterSize = 41;
    bmState->preFilterCap = 31;
    bmState->SADWindowSize = 41;
    bmState->minDisparity = -64;
    bmState->numberOfDisparities = 128;
    bmState->textureThreshold = 10;
    bmState->uniquenessRatio = 15;
  }
  else
  {
      int nStereoPreFilterType = CV_STEREO_BM_NORMALIZED_RESPONSE;
      int nStereoPreFilterSize= 20;//33;
      int nStereoPreFilterCap=19;
      int nStereoSADWindowSize=20;//31;//22;
      int nStereoMinDisparity =4;//6; //this is multiplied by -16
      int nStereoNumDisparities =4;//10;//14; //this is multiplied by 16
      int nStereoTextureThreshold =12;
      int nStereoUniquenessRatio =3;//9;
      int nStereoFillUnmatchedRegions = 1;
      int nStereoSpeckleWindowSize = 0;
      int nStereoSpeckleRange = 0;

      if( nStereoPreFilterSize % 2 == 0 ) nStereoPreFilterSize++;
      if( nStereoPreFilterSize < 5 ) nStereoPreFilterSize = 5;
      if( nStereoPreFilterSize > 255) nStereoPreFilterSize = 255;
      if( nStereoPreFilterCap < 1 ) nStereoPreFilterCap = 1;
      if( nStereoPreFilterCap > 63 ) nStereoPreFilterCap = 63;
      if( nStereoSADWindowSize % 2 == 0 ) nStereoSADWindowSize++;
      if( nStereoSADWindowSize < 5 ) nStereoSADWindowSize = 5;
      if( nStereoSADWindowSize > 255 )nStereoSADWindowSize = 255;
      if( nStereoSADWindowSize >= MIN(imageSize.width, imageSize.height) )
      nStereoSADWindowSize = MIN(imageSize.width, imageSize.height);
      if( nStereoMinDisparity < 0) nStereoMinDisparity=0;
      if( nStereoNumDisparities <1 ) nStereoNumDisparities=1;
      if( nStereoTextureThreshold < 0 ) nStereoTextureThreshold = 1;
      if( nStereoUniquenessRatio < 0 ) nStereoUniquenessRatio = 1;

      bmState->speckleWindowSize = nStereoSpeckleWindowSize;
      bmState->speckleRange = nStereoSpeckleRange;
      bmState->preFilterSize=nStereoPreFilterSize; // 41
      bmState->preFilterCap=nStereoPreFilterCap; //31
      bmState->SADWindowSize=nStereoSADWindowSize; //41
      bmState->minDisparity=nStereoMinDisparity*-16;//-64
      bmState->numberOfDisparities=nStereoNumDisparities*16; //128
      bmState->textureThreshold=nStereoTextureThreshold; //10
      bmState->uniquenessRatio=nStereoUniquenessRatio; //15
  }
  
  // rectify images
  cvRemap(leftGray, img1r, mx1, my1);
  cvRemap(rightGray, img2r, mx2, my2);
 
  // find correspondence
  cvFindStereoCorrespondenceBM(img1r, img2r, disp, bmState);
  //cvFindStereoCorrespondenceBM(leftGray, rightGray, disp, bmState);
  cvNormalize(disp, vdisp, 0, 256, CV_MINMAX);
//   cvConvertScale(disp, disp, -16);
  
//  q[0][3] = -m1[0][2];
//  q[1][2] = m1[0][0] / m1[1][1];
//  q[1][3] = -m1[1][2] * m1[0][0] / m1[1][1];
//  q[2][3] = m1[0][0];
//  q[3][2] = -1 / trans[0];
//  q[3][3] = -(m1[0][2] - m2[0][2]) / trans[0];
    
  cvReprojectImageTo3D(disp, result3DImage, &_q);
  
  int height     = result3DImage->height;
  int width      = result3DImage->width;
  int step       = result3DImage->widthStep/sizeof(float);
  int channels   = result3DImage->nChannels;
  float *data    = (float *)result3DImage->imageData;
  float min, max, max2;
  float w;
  int countNumPoints;
  countNumPoints = 0;
  min = 1000000;
  max = -1000000;
  max2 = -1000000;
    
    // Draw a line to test the retification
//   for(i = 0; i < 240; i += 10)
//   {
//     cvLine(img1r, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
//     cvLine(img2r, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
//   }  
//   
//   for(i = 0; i < 240; i += 20)
//   {
//     cvLine(leftGray, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
//     cvLine(rightGray, cvPoint(0,i), cvPoint(319, i), cvScalar(255, 0, 0), 0, 0);
//   }
//   
//   for(i = 0; i < 360; i += 20)
//   {
//     cvLine(leftGray, cvPoint(i,0), cvPoint(i, 239), cvScalar(255, 0, 0), 0, 0);
//     cvLine(rightGray, cvPoint(i,0), cvPoint(i, 239), cvScalar(255, 0, 0), 0, 0);
//   } 
    
  // Show the openCv images
  cvShowImage("OriginalLeft", leftGray);
  cvShowImage("GrayLeft", img1r);
  cvShowImage("OriginalRight", rightGray);
  cvShowImage("GrayRight", img2r);  
  
  //cvShowImage("Disparity", disp);
  
  //-------------start print-----------------------
  
  cvShowImage("Disparity", vdisp);
  
  cout << "M1" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << m1[i][j] << " ";
    }
    cout << endl;
  }
  
  cout << "M2" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << m2[i][j] << " ";
    }
    cout << endl;
  }

  cout << "D1" << endl;
  for(j = 0; j < 5; j++)
  {
    cout << d1Zeros[j] << " ";
  }
  cout << endl;
  
  cout << "D2" << endl;
  for(j = 0; j < 5; j++)
  {
    cout << d2Zeros[j] << " ";
  }
  cout << endl;
  
  cout << "R" << endl;
  for(i = 0; i < 3; i++)
  {
    for(j = 0; j < 3; j++)
    {
      cout << rot[i][j] << " ";
    }
    cout << endl;
  }
  
  cout << "T" << endl;
  for(j = 0; j < 3; j++)
  {
    cout << trans[j] << " ";
  }
  cout << endl;
  
  cout << "Q" << endl;
  for(i = 0; i < 4; i++)
  {
    for(j = 0; j < 4; j++)
    {
      cout << q[i][j] << " ";
    }
    cout << endl;
  }
  //-------------end print----------------------------
  
  int key;
  
  while (key != 'q')
    key = cvWaitKey(100);
  
  //extracting depth
  imageX = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  imageY = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  imageZ = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  cvSplit(result3DImage, imageX, imageY, imageZ, NULL);
  IplImage *imageZu = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
  cvNormalize(imageZ, imageZu, 0, 256, CV_MINMAX);
  
  cvShowImage("Disparity", imageZu);
  
  while (key != 't')
    key = cvWaitKey(100);
  
  countNumPoints = imageSize.width * imageSize.height;
  double object_points[countNumPoints][3];
  CvMat _object_points = cvMat(countNumPoints, 3, CV_64F, object_points);
  double image_points[countNumPoints][2];
  CvMat _image_points = cvMat(countNumPoints, 2, CV_64F, image_points);
  
  double obRot[3];
  CvMat _obRot = cvMat(3, 1, CV_64F, obRot);
  double obTran[3];
  CvMat _obTran = cvMat(3, 1, CV_64F, obTran);
  
  for(i = 0; i < 3; i++)
  {
    obRot[i] = 0;
    obTran[i] = 0;
  }
  
  double qInv[4][4];
  CvMat _qInv = cvMat(4, 4, CV_64F, qInv);

  cvInvert(&_q, &_qInv);
  
  angle = 0;
  
  rotX[0][0] = 1.0;
  rotX[0][1] = 0.0;
  rotX[0][2] = 0.0;
  rotX[1][0] = 0.0;
  rotX[1][1] = cos(angle*PI/180);
  rotX[1][2] = sin(angle*PI/180);
  rotX[2][0] = 0.0;
  rotX[2][1] = -sin(angle*PI/180);
  rotX[2][2] = cos(angle*PI/180);
  
//   angle = -7.1;
  angle = 5;

  rotY[0][0] = cos(angle*PI/180);
  rotY[0][1] = 0.0;
  rotY[0][2] = -sin(angle*PI/180);
  rotY[1][0] = 0.0;
  rotY[1][1] = 1.0;
  rotY[1][2] = 0.0;
  rotY[2][0] = sin(angle*PI/180);
  rotY[2][1] = 0.0;
  rotY[2][2] = cos(angle*PI/180);
  
//   angle = 0;
  angle = 0;
  
  rotZ[0][0] = cos(angle*PI/180);
  rotZ[0][1] = sin(angle*PI/180);
  rotZ[0][2] = 0.0;
  rotZ[1][0] = -sin(angle*PI/180);
  rotZ[1][1] = cos(angle*PI/180);
  rotZ[1][2] = 0.0;
  rotZ[2][0] = 0.0;
  rotZ[2][1] = 0.0;
  rotZ[2][2] = 1.0;
  
  rotzy = cvCreateMat( 3, 3, CV_64FC1);
  
  cvMatMul(&_rotZ, &_rotY, rotzy);
  cvMatMul(rotzy, &_rotX, &_rot);
  
  float rotx, roty, rotz;
  int newx, newy;
  
  IplImage *rotatedImage = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);  
  uchar *data2 = (uchar *)rotatedImage->imageData;
  float *data3 = (float *)result2DPoints->imageData;
  int step2       = rotatedImage->widthStep/sizeof(uchar);
  int channels2   = rotatedImage->nChannels;
  int step3       = result2DPoints->widthStep/sizeof(float);
  int channels3   = result2DPoints->nChannels;
  
  int stepX       = imageX->widthStep/sizeof(float);
  float *dataX    = (float *)imageX->imageData;
  int stepY       = imageY->widthStep/sizeof(float);
  float *dataY    = (float *)imageY->imageData;
  int stepZ       = imageZ->widthStep/sizeof(float);
  float *dataZ    = (float *)imageZ->imageData;
  
  int countNumPoints2;
  countNumPoints2 = 0;
  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
    {
      if (/*dataZ[i * stepZ + j] > -7000*/1)
      {
// 	data[i * step + j * channels + 0] -= 68;
// 	data[i * step + j * channels + 2] -= 55;
	
	rotx = (rot[0][0] * dataX[i * stepX + j]) +
	       (rot[0][1] * dataY[i * stepY + j]) +
	       (rot[0][2] * dataZ[i * stepZ + j]);
	roty = (rot[1][0] * dataX[i * stepX + j]) +
	       (rot[1][1] * dataY[i * stepY + j]) +
	       (rot[1][2] * dataZ[i * stepZ + j]);
	rotz = (rot[2][0] * dataX[i * stepX + j]) +
	       (rot[2][1] * dataY[i * stepY + j]) +
	       (rot[2][2] * dataZ[i * stepZ + j]);
	data[i * step + j * channels + 0] = rotx;
	data[i * step + j * channels + 1] = roty;
	data[i * step + j * channels + 2] = rotz;
	
// 	data[i * step + j * channels + 0] += 68;
// 	data[i * step + j * channels + 2] += 55;
	
// 	object_points[i * width + j][0] = dataX[i * stepX + j];
// 	object_points[i * width + j][1] = dataY[i * stepY + j];
// 	object_points[i * width + j][2] = dataZ[i * stepZ + j];
      }
    }
  
//   for(i = 0; i < 5; i++)
//     d1[i] = 0.0;
//   for(i = 0; i < 3; i++)
//     for(j = 0; j < 3; j++)
//       m1[i][j] = p1[i][j];
//     
//   cvProjectPoints2(&_object_points, &_obRot, &_obTran, &_m1, &_d1Zeros, &_image_points);
//   
//   countNumPoints2 = 0;
//   for(i = 0; i < height; i++)
//     for(j = 0; j < width; j++)
//     {
//       newx = image_points[countNumPoints2][0];
//       newy = image_points[countNumPoints2++][1];
//       data2[newy * step2 + newx * channels2 + 0] = CV_MAT_ELEM(*img1r, double, i, j);    
//     }
    
  cvPerspectiveTransform(result3DImage, result2DPoints, &_qInv);  
  
  imageX = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  imageY = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  imageZ = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
  cvSplit(result3DImage, imageX, imageY, imageZ, NULL);
  
  for(i = 0; i < height; i++)
    for(j = 0; j < width; j++)
    {
      newx = data3[i * step3 + j * channels3 + 0];
      newy = data3[i * step3 + j * channels3 + 1];
      if(newy >= 0 && newy < height && newx >= 0 && newx < width)
	data2[newy * step2 + newx] = CV_MAT_ELEM(*img1r, uchar, i, j);
    }
  
  cvSave("result3dImage.txt", imageZu);
  
  yarp::sig::file::read(tempYImage, "/home/nigno/Images/dataset2/Left45.ppm");
  
  // Convert the image in a openCv image
  leftImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
				 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), leftImg, CV_RGB2BGR);
  
  leftGray = cvCreateImage(cvGetSize(leftImg), 8, 1);
  
  cvCvtColor(leftImg, leftGray, CV_BGR2GRAY);
  
  tempDistImage = cvCloneImage(leftGray);
  cvRemap(tempDistImage, leftGray, mapXDis, mapYDis);
  
  cvRemap(leftGray, img1r, mx1, my1);
  
  yarp::sig::file::read(tempYImage, "/home/nigno/Images/dataset2/Right45.ppm");
  
  // Convert the image in a openCv image
  rightImg = cvCreateImage(cvSize(tempYImage.width(),  
				 tempYImage.height()), 
				 IPL_DEPTH_8U, 3 );
  
  cvCvtColor((IplImage*)tempYImage.getIplImage(), rightImg, CV_RGB2BGR);
  
  rightGray = cvCreateImage(cvGetSize(rightImg), 8, 1);
  
  cvCvtColor(rightImg, rightGray, CV_BGR2GRAY);
  
  tempDistImage = cvCloneImage(rightGray);
  cvRemap(tempDistImage, rightGray, mapXDis2, mapYDis2);
  
  cvRemap(rightGray, img2r, mx2, my2);
  
  diff = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
  cvAbsDiff (img1r, rotatedImage, diff);
  cvThreshold(diff, diff, 40, 255, CV_THRESH_BINARY);
  
  cvShowImage("GrayLeft", img1r);
  cvShowImage("GrayRight", diff);
  
  // show the disparity map
  cvShowImage("Disparity", rotatedImage);
  
  while (key != 'q')
    key = cvWaitKey(100);
  
  // ciclo di rotazioni
  
//   for(k = 0; k < 100; k += 5)
//   {
//     
//     for(i = 0; i < height; i++)
//       for(j = 0; j < width; j++)
// 	data2[i * step2 + j * channels2 + 0] = 0;
//     
//     angle = 0;
//   
//     rotX[0][0] = 1.0;
//     rotX[0][1] = 0.0;
//     rotX[0][2] = 0.0;
//     rotX[1][0] = 0.0;
//     rotX[1][1] = cos(angle*PI/180);
//     rotX[1][2] = sin(angle*PI/180);
//     rotX[2][0] = 0.0;
//     rotX[2][1] = -sin(angle*PI/180);
//     rotX[2][2] = cos(angle*PI/180);
//   
// //   angle = -7.1;
//     angle = 1;
// 
//     rotY[0][0] = cos(angle*PI/180);
//     rotY[0][1] = 0.0;
//     rotY[0][2] = -sin(angle*PI/180);
//     rotY[1][0] = 0.0;
//     rotY[1][1] = 1.0;
//     rotY[1][2] = 0.0;
//     rotY[2][0] = sin(angle*PI/180);
//     rotY[2][1] = 0.0;
//     rotY[2][2] = cos(angle*PI/180);
//   
// //   angle = 0;
//     angle = 0;
//   
//     rotZ[0][0] = cos(angle*PI/180);
//     rotZ[0][1] = sin(angle*PI/180);
//     rotZ[0][2] = 0.0;
//     rotZ[1][0] = -sin(angle*PI/180);
//     rotZ[1][1] = cos(angle*PI/180);
//     rotZ[1][2] = 0.0;
//     rotZ[2][0] = 0.0;
//     rotZ[2][1] = 0.0;
//     rotZ[2][2] = 1.0;
//   
//     rotzy = cvCreateMat( 3, 3, CV_64FC1);
//   
//     cvMatMul(&_rotZ, &_rotY, rotzy);
//     cvMatMul(rotzy, &_rotX, &_rot);
// 
//     for(i = 0; i < height; i++)
//       for(j = 0; j < width; j++)
//       {
// 	if (/*dataZ[i * stepZ + j] > -7000*/1)
// 	{
// 	  rotx = (rot[0][0] * data[i * step + j * channels + 0]) +
// 		 (rot[0][1] * data[i * step + j * channels + 1]) +
// 		 (rot[0][2] * data[i * step + j * channels + 2]);
// 	  roty = (rot[1][0] * data[i * step + j * channels + 0]) +
// 		 (rot[1][1] * data[i * step + j * channels + 1]) +
// 		 (rot[1][2] * data[i * step + j * channels + 2]);
// 	  rotz = (rot[2][0] * data[i * step + j * channels + 0]) +
// 		 (rot[2][1] * data[i * step + j * channels + 1]) +
// 		 (rot[2][2] * data[i * step + j * channels + 2]);
// 	  data[i * step + j * channels + 0] = rotx;
// 	  data[i * step + j * channels + 1] = roty;
// 	  data[i * step + j * channels + 2] = rotz;
//       }
//     }
//     
//     cvPerspectiveTransform(result3DImage, result2DPoints, &_qInv);  
//   
//     imageX = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
//     imageY = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
//     imageZ = cvCreateImage(imageSize, IPL_DEPTH_32F, 1);
//     cvSplit(result3DImage, imageX, imageY, imageZ, NULL);
//   
//     for(i = 0; i < height; i++)
//       for(j = 0; j < width; j++)
//       {
// 	newx = data3[i * step3 + j * channels3 + 0];
// 	newy = data3[i * step3 + j * channels3 + 1];
// 	if(newy >= 0 && newy < height && newx >= 0 && newx < width)
// 	  data2[newy * step2 + newx] = CV_MAT_ELEM(*img1r, uchar, i, j);
//       }
//       
//     key = cvWaitKey(100);
//   
//     // show the disparity map
//     cvShowImage("Disparity", rotatedImage);
//     
//   }
  
  cvDestroyWindow("OriginalLeft");
  cvDestroyWindow("GrayLeft");
  cvDestroyWindow("OriginalRight");
  cvDestroyWindow("GrayRight");
  cvDestroyWindow("Disparity");
  
  cvReleaseImage(&tempCImage);
  cvReleaseImage(&gray);
  cvReleaseImage(&leftImg);
  cvReleaseImage(&rightImg);
  cvReleaseImage(&leftGray);
  cvReleaseImage(&rightGray);
//   cvReleaseImage(&imageX);
//   cvReleaseImage(&imageY);
//   cvReleaseImage(&imageZ);
//   cvReleaseImage(&rotatedImage);
//   cvReleaseImage(&result3DImage);
//   cvReleaseImage(&result2DPoints);
//   cvReleaseImage(&mapXDis);
//   cvReleaseImage(&mapXDis2);
//   cvReleaseImage(&mapYDis);
//   cvReleaseImage(&mapYDis2);
//   cvReleaseImage(&tempDistImage);
  
  
  cvReleaseStereoBMState(&bmState);
  
  cvReleaseMat(&mx1);
  cvReleaseMat(&my1);
  cvReleaseMat(&mx2);
  cvReleaseMat(&my2);
  cvReleaseMat(&img1r);
  cvReleaseMat(&img2r);
  cvReleaseMat(&disp);
  cvReleaseMat(&vdisp);
  cvReleaseMat(&pair);
  cvReleaseMat(&rotzy);
  
  return 0;
}

bool fileExists(string strFilename) { 
  struct stat stFileInfo; 
  bool blnReturn; 
  int intStat; 

  intStat = stat(strFilename.c_str(),&stFileInfo); 
 
  if(intStat == 0) 
  { 
    blnReturn = true; 
  } 
  else 
  { 
    blnReturn = false; 
  } 
   
  return(blnReturn); 
}



